/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "RangingFileWriter.hpp"
CRangingFileWriter::CRangingFileWriter()
  :iFile(NULL)
{

}
//*****************************************************************************

CRangingFileWriter::~CRangingFileWriter()
{
  CloseFile();
}
//*****************************************************************************
 
void CRangingFileWriter::OpenFile(std::string aFileName,
                                  std::string aMaCIGroup,
                                  std::string aInstanceName,
                                  TRangingHeader aHeader)
{
  iFile = fopen(aFileName.c_str(),"wt");
  fprintf(iFile,"%%MaCI_Ranging log file\n");
  fprintf(iFile,"%%Group: %s Instance: %s\n",aMaCIGroup.c_str(),aInstanceName.c_str());
  fprintf(iFile,"%%");
  fprintf(iFile,"timestamp_s timestamp_us ");
  fprintf(iFile,"beam_width beam_count ");
  fprintf(iFile,"angle ");
  fprintf(iFile,"distance ");
  fprintf(iFile,"angle ");
  fprintf(iFile,"distance ");
  fprintf(iFile,"... \n");
  fprintf(iFile,"%%");
  fprintf(iFile,"maxDistance: %f, minDistance: %f",aHeader.maxDistance,aHeader.minDistance);
  fprintf(iFile," positionX %f, positionY %f, positionZ %f, heading %f", 
          aHeader.positionX,aHeader.positionY,aHeader.positionZ,aHeader.heading);
  fprintf(iFile,"\n");
}
//*****************************************************************************

void CRangingFileWriter::CloseFile()
{
  if(iFile){
    fclose(iFile);
  
    iFile = NULL;
  }
}
//*****************************************************************************

bool CRangingFileWriter::WriteLine(MaCI::Ranging::TDistanceHeader &aHeader,
                                   MaCI::Ranging::TDistanceArray &aArray,
                                   MaCI::Common::TTimestamp &aTimestamp)
{

  //Timestamp and header
  fprintf(iFile,"%u %u %f %u ",aTimestamp.timestamp_s,aTimestamp.timestamp_us,aHeader.beam_width,aHeader.beam_count);

  
  for(unsigned int i = 0; i< aHeader.beam_count;i++){
    fprintf(iFile,"%f %f ",aArray[i].angle, aArray[i].distance);


  }

  fprintf(iFile,"\n");
  fflush(iFile);
  return true;
}
//*****************************************************************************

